﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using TinyROS.Messages;
using TinyROS.Exceptions;

namespace TinyROS
{
    public abstract class MuxNode<TMessage> : Node, ICommandInteractive where TMessage : Message
    {
        public abstract void AddInput(string topicName, int priority);
        public abstract void ExecuteCommand(string cmd, bool raiseError = true);
        public abstract void ExecuteCommandMute(string cmd, bool raiseError = true);
        protected abstract void MuxHandler(TMessage msg, int priority);
    }
    public class DiffcarCmdMuxByTimeNode : MuxNode<DiffcarCourseMessage>
    {
        public double Timeout { get; set; } = 3.0d;
        private Publisher<DiffcarCourseMessage> VelPublisher;
        private List<int> Priorities = new();
        private Dictionary<int, DiffcarCourseMessage> Inputs = new();
        private Dictionary<int, Subscriber> Subscibers = new();
        private Dictionary<Publisher, string> pubedTopicName = new();
        private Dictionary<Subscriber, string> subedTopicName = new();
        private SystemCore master;
        public override Dictionary<Publisher, string> PublishedTopicName { get => this.pubedTopicName; }
        public override Dictionary<Subscriber, string> SubscribedTopicName { get => this.subedTopicName; }
        private bool running = false;
        public override bool IsRunning => this.running;

        public DiffcarCmdMuxByTimeNode() : 
            this(TinyROSConfig.DefaultTopicName.DiffcarCommand)
        {

        }
        public DiffcarCmdMuxByTimeNode(string velPubTopicName)
        {
            this.VelPublisher = new();
            this.PublishedTopicName.Add(this.VelPublisher, velPubTopicName);
        }
        public override void AddInput(string topicName, int priority)
        {
            if(this.Inputs.ContainsKey(priority))
            {
                throw new ArgumentException();
            }
            Subscriber<DiffcarCourseMessage> subscriber = new(
                (DiffcarCourseMessage msg) =>
                {
                    this.MuxHandler(msg, priority: priority);
                });
            this.Priorities.Add(priority);
            this.Priorities.Sort();
            this.Inputs.Add(priority, null);
            this.Subscibers.Add(priority, subscriber);
            this.SubscribedTopicName.Add(subscriber, topicName);
            if(this.Initialized)
            {
                var topic = this.master.RequestTopic<DiffcarCourseTopic>(topicName);
                subscriber.SetTopic(topic);
            }
        }

        public override void Init(SystemCore master)            
        {
            this.master = master;
            foreach(var rawsuber in this.Subscibers.Values)
            {
                var velCmdTopic = master.RequestTopic<DiffcarCourseTopic>(this.SubscribedTopicName[rawsuber]);
                var suber = rawsuber as Subscriber<DiffcarCourseMessage>;
                suber.SetTopic(velCmdTopic);
            }
            var velCmdPubTopic = master.RequestTopic<DiffcarCourseTopic>(this.PublishedTopicName[this.VelPublisher]);
            this.VelPublisher.SetTopic(velCmdPubTopic);
            this.Initialized = true;
        }


        public override void Run()
        {
            if(!this.Initialized)
            {
                throw new NodeNotInitedException();
            }
            foreach(var rawSuber in this.Subscibers.Values)
            {
                var suber = rawSuber as Subscriber<DiffcarCourseMessage>;
                suber.Start(Subscriber<DiffcarCourseMessage>.SubscribeType.Passive);
                
            }
            this.running = true;
        }

        public override void Stop()
        {
            foreach(var rawSuber in this.Subscibers.Values)
            {
                var suber = rawSuber as Subscriber<DiffcarCourseMessage>;
                suber.Stop();
            }
            this.running = false;
        }
        protected override void MuxHandler(DiffcarCourseMessage msg, int priority)
        {
            this.Inputs[priority] = msg;
            this.TryPublish(priority);
            //this.CheckAfter(this.Timeout);
            //Console.WriteLine($"{priority} received");
        }
        private void TryPublish(int priority)
        {
            // 尝试发布指定优先级的消息
            // 如果该优先级的消息有效，且更高优先级的消息均无效，则发布它，否则无事发生
            // 如果指定优先级为-1，则发布遇到的第一个有效的消息
            foreach(var i in this.Priorities)
            {
                if(this.Inputs[i] == null)
                {
                    //Console.WriteLine($"{i} is null");
                    if (i == priority)
                    {
                        return;
                    }
                    else
                    {
                        continue;
                    }                   
                }
                if(this.IsValid(this.Inputs[i]))
                {
                    if (i == priority || i == -1) 
                    {
                        this.VelPublisher.Talk(this.Inputs[i]);
                        //Console.WriteLine($"{i} pubed");
                        return;
                    }
                    else 
                    {
                        return;
                    }
                }
                //Console.WriteLine($"{i} is not valid");
            }
        }
        private bool IsValid(DiffcarCourseMessage msg)
        {
            long t = DateTime.Now.Ticks;
            long diff = t - msg.Header.Time.Ticks;
            TimeSpan diffSpan = new(diff);
            var sec = diffSpan.TotalSeconds;
            return sec <= this.Timeout;
        }
        private async void CheckAfter(double t)
        {
            await Task.Delay((int)(t * 1000) + 100);
            this.TryPublish(-1);
        }

        public override void ExecuteCommand(string cmd, bool raiseError = true)
        {
            try
            {
                var args = cmd.Split(" ", 3, StringSplitOptions.RemoveEmptyEntries);
                if(args.Length != 3)
                {
                    throw new ArgumentCountException();
                }
                else if(args[0]!= "addinput")
                {
                    throw new CommandNotFoundException(cmd);
                }
                else
                {
                    string topicName = args[1];
                    int priority = int.Parse(args[2]);
                    this.AddInput(topicName, priority);
                    SystemCore.ConsoleWriteLineWithColor($"成功添加输入,优先级{priority}", ConsoleColor.White);
                }
            }
            catch
            {
                throw;
            }
        }

        public override void ExecuteCommandMute(string cmd, bool raiseError = true)
        {
            
        }
    }
    public abstract class ConstSpeedNode : Node
    {

    }
    public class ConstDiffcarSpeedNode : ConstSpeedNode
    {
        public int Period { get; set; } = 1500;
        private CancellationTokenSource Canceller;
        private Publisher<DiffcarCourseMessage> ConstSpeedPublisher;
        private DiffcarCourse ConstCourse = new(0.0f, 0.0f);
        private Dictionary<Publisher, string> pubedTopicName = new();
        private Dictionary<Subscriber, string> subedTopicName = new();
        public override Dictionary<Publisher, string> PublishedTopicName =>  this.pubedTopicName;
        public override Dictionary<Subscriber, string> SubscribedTopicName => this.subedTopicName;
        private bool running = false;
        public override bool IsRunning => this.running;

        public ConstDiffcarSpeedNode(string topicName)
        {
            this.ConstSpeedPublisher = new();
            this.PublishedTopicName.Add(this.ConstSpeedPublisher, topicName);
        }
        public void SetConstCourseValue(float v, float r)
        {
            this.ConstCourse = new(v, r);
        }
        public void SetConstCourseValue(DiffcarCourse course)
        {
            this.ConstCourse = course;
        }
        public override void Init(SystemCore master)
        {
            var topic = master.GetTopic(this.PublishedTopicName[this.ConstSpeedPublisher]) as DiffcarCourseTopic;
            if (topic == null)
            {
                throw new TopicNotExistException();
            }
            else
            {
                this.ConstSpeedPublisher.SetTopic(topic);
            }
            this.Initialized = true;
        }

        public override void Run()
        {
            if(!this.Initialized)
            {
                throw new NodeNotInitedException();
            }
            else
            {
                this.Canceller = new();
                this.KeepPublish();
                this.running = true;
            }
        }

        public override void Stop()
        {
            try
            {
                this.Canceller.Cancel();
            }
            catch
            {

            }
            this.running = false;
        }
        private async void KeepPublish()
        {
            try
            {
                await Task.Run(async () =>
                {
                    while (true)
                    {
                        this.Canceller.Token.ThrowIfCancellationRequested();
                        try
                        {
                            var msg = new DiffcarCourseMessage(this.ConstCourse, this);
                            this.ConstSpeedPublisher.Talk(msg);
                            await Task.Delay(this.Period);
                        }
                        catch
                        {

                        }
                    }
                }, this.Canceller.Token);
            }
            catch(OperationCanceledException)
            {

            }
        }
    }
    public static class MessageValidator
    {
        public static bool DiffcarCourseValidator(DiffcarCourseMessage msg)
        {
            var v = msg.Value.Linear;
            var r = msg.Value.TurningRadius;
            return v < 1e-5f && r < 1e-5f;
               
        }
    }
}
